/*
 * ************************************************
 *
 *              STM32 blink gcc demo
 *
 *  CPU: STM32F103C8
 *  PIN: PA1
 *
 * ************************************************
 */

#include "stm32f10x.h"
#include <stdio.h>
#include "Delay.h"
#include "main.h"
#include "OLED.h"
#include "usart.h"

#ifdef REMOTE_CONTROLL
#include "nrf24l01.h"
#include "led.h"
#include "rtc.h"
#include "Servo_motor.h"
#include "map.h"
#endif


#ifdef TEST_PID
#include "pwm_AT8236.h"
#include "encoder_GMR.h"
#include "motor_AT8236.h"
#include "pid_AT8236.h"
#endif

#ifdef OPEN_LOOP
#include "motor_AT8236.h"
#include "pwm_AT8236.h"
#endif

#ifdef REMOTE_CONTROLL
void PWM_reset(void);

uint16_t PWMvalue[SBUS_CHANNEL_NUMBER];
uint16_t recPWMvalue[SBUS_CHANNEL_NUMBER];
uint8_t chPacket[32];
uint16_t i=0, startIndex = 0;
uint8_t signalLoss = 0;         // 1表示信号丢失
uint32_t lastTime = 0;
uint32_t sbusTime = 0;
#endif

#ifdef TEST_PID

int32_t TargetVelocity = 20;
float Velocity_Kp = 3, Velocity_Ki = 1, Velocity_Kd = 0;
int32_t encoder_A = 0, encoder_B = 0;
int32_t Velocity_PWMA = 0,Velocity_PWMB = 0; 
char str[30];
#endif

#ifdef OPEN_LOOP
int32_t TargetVelocity = 0;
#endif 

int main()
{
    SystemInit();
#ifdef REMOTE_CONTROLL
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
    PWM_reset();
    // Serial_Init();
    rtc_Init();
    LED_Init();
    NRF24L01_Init();
    while (NRF24L01_Check()) {
        Delay_ms(200);
    }
    
    NRF24L01_RX_Mode();

    // 舵机使用 TIM1
    Servo_Init_TIM1(19999, 71); // 预分频72， 频率1MHz, 周期1us；自动装载值 20 000， 故 PWM周期 1us *20 000
    
#endif

#ifdef OPEN_LOOP
    TIM3_PWM_Init(7199, 0);     // 定时器3，CH1~4， 输出到AT8236
    OLED_Init();
    OLED_ShowString(1, 1, "CH1:");
    OLED_ShowString(2, 1, "CH3:");
    OLED_ShowString(3, 1, "PWM:");
#endif
    
#ifdef TEST_PID
    TIM3_PWM_Init(7199, 0);     // 定时器3，CH1~4， 输出到AT8236

    Encoder_Init_Tim4();        // 编码器A, B6 B7
    Encoder_Init_Tim2();        // 编码器B, A0 A1

    OLED_Init();
    // USART1_Init(9600);

    OLED_ShowString(1, 1, "CH1:");
    OLED_ShowString(2, 1, "CH3:");
    OLED_ShowString(3, 1, "EncA:");
    OLED_ShowString(4, 1, "PWMA:");

#endif

    while (1) 
    {
#ifdef REMOTE_CONTROLL
        if (NRF24L01_RxPacket(chPacket) == 0) {
            for (i=0; i<chNum*2; i++)       // 寻找起始位置， 一般在第2~3个元素
            {
                if(chPacket[i] == 0x00 && chPacket[i+1] == 0x00) {
                    startIndex = i+2;
                    break;
                }
            }
            for(i=0; i<chNum; i++)          // 从起始位置开始处理
            {
                recPWMvalue[i] = (uint16_t)chPacket[startIndex] << 8 | (uint16_t)(chPacket[startIndex+1]);  // 合并u8为u16
                if (recPWMvalue[i] != 0) PWMvalue[i] = recPWMvalue[i];
                startIndex = startIndex + 2;
            }
            for (i=chNum; i<16; i++) {
                PWMvalue[i] = 1500;
            }
            signalLoss = 0;     // 信号已连接标志
            LED_ON();
            lastTime = nowTime;
        }
        
        // if (nowTime > sbusTime) {} // 输出SBUS
    
        if (nowTime - lastTime > 1) // 距离上次接收时间大于1s, 则说明失去信号
        {
            LED_OFF();
            signalLoss = 1;
            PWM_reset();
            Delay_ms(200);
        }

        // ppm data convert
        // for (i=0; i<chNum; i++) {/* chTime[i] = PWMvalue[i]*9; */ }

        TIM_SetCompare4(TIM1, PWMvalue[0]); // 输出给PWM-PA11 舵机控制

        if (PWMvalue[2] > 1520) {
            TargetVelocity = (int32_t)map((float)PWMvalue[2], 1520, 2000, 3000, 7200);
        }

        if (PWMvalue[2] < 1480) {
            TargetVelocity = (int32_t)map((float)PWMvalue[2], 1000, 1480, -7200, -3000);
        }

        OLED_ShowNum(1, 12, PWMvalue[0], 5);
        OLED_ShowSignedNum(2, 11, PWMvalue[2], 5);
        OLED_ShowSignedNum(3, 11, TargetVelocity, 5);
#endif

#ifdef OPEN_LOOP
        motoA(TargetVelocity);
        motoB(TargetVelocity);
#endif

#ifdef TEST_PID
        encoder_A = (int)(Read_Encoder(4) / 10);
        encoder_B = (int)(Read_Encoder(2) / 10);
        // sprintf(str, "encoder A: %d, B: %d", encoder_A, encoder_B);
        // printf(str);

        // OLED_ShowSignedNum(1, 11, encoder_A, 5);
        // OLED_ShowSignedNum(2, 11, encoder_B, 5);

        Velocity_PWMA = Velocity_A(TargetVelocity, encoder_A);
        Velocity_PWMB = Velocity_B(TargetVelocity, encoder_B);

        // OLED_ShowSignedNum(3, 10, Velocity_PWMA, 6);
        // OLED_ShowSignedNum(4, 10, Velocity_PWMB, 6);

        motoA(Velocity_PWMA);
        motoB(Velocity_PWMB);

        OLED_ShowSignedNum(3, 10, encoder_A, 6);
        OLED_ShowSignedNum(4, 10, Velocity_PWMA, 6);

#endif
    }
}

#ifdef REMOTE_CONTROLL
void PWM_reset(void)
{
    PWMvalue[0] = 1500; // 通道1置中
    PWMvalue[1] = 1500; // 通道2置中
    PWMvalue[2] = 1500; // 油门最低
    PWMvalue[3] = 1500; // 通道1置中
    for (i=0; i<chNum && i> 4; i++) {
        PWMvalue[i] = 1500;
    }
}
#endif
